#include <stdio.h>
#include <arm_neon.h>
#include "NeonFunctions.h"

/**
 * Funcao que converte YUV4:2:2 para RGB24 utilizando as instrucoes NEON
 * */
void v4lconvert_yuyv_to_rgb24_optimized_NEON_final(const unsigned char *src, unsigned char *dest,
  int width, int height)
{
	int j,i=0;
	int x;
	int screen_size_pixels = width*height;//1280x720
	int screen_size_bytes = screen_size_pixels*2;
	
	const uint8_t *pointer_y = src;
	const uint8_t *pointer_cbcr = src;
	uint8_t *vdest = dest; //ponteiro para gravar no destino
	
	uint8x8x2_t y; //captura os y`s e os cbs e crs
	uint8x8x4_t yuyv; //armazena os 4 componentes porem utiliza apenas os cbs e crs
	uint8x8x3_t rgb; //armazena os 3 componentes finais para a gravacao
	
	int16x4x2_t utemp; //variavel temporaria para o u, utilizada para intercalar os valores
	int16x4x2_t vtemp; //variavel temporaria para o v, utilizada para intercalar os valores

	int16x8_t u; //u ou cb
	int16x8_t v; //v ou cr
	
	
	int16x8_t vIntermediario; //armazena valores intermediarios
	int16x8_t uIntermediario;//armazena valores intermediarios
	int16x8_t v1;
	int16x8_t u1;
	int16x8_t rgIntermediario;//armazena valores intermediarios
	int16x8_t rg;
	int16x8_t red;
	int16x8_t green;
	int16x8_t blue;
	uint8x8_t convert;
	uint8x8x2_t final;
	
	/* vetores de constantes*/
	int16x8_t vector128 = vdupq_n_s16(-128);
	int16x8_t vector0 = vdupq_n_s16(0);
	int16x8_t vector255 = vdupq_n_s16(255);
	
	int16x8_t y0;
	int16x8_t yuv1;
	int16x8_t yuv3; 
    for (j = 0; j < screen_size_bytes; j += 32) {

		/* Apenas os cbs e crs sao utilizados*/
		yuyv = vld4_u8(pointer_cbcr);
		
		yuv1 = vmovl_u8(yuyv.val[1]);
		yuv3 = vmovl_u8(yuyv.val[3]);
		
		for(i = 0;i<2;i++){
			rgb = vld3_u8(vdest); //carrega rgb para os registradores (3)
			/* Apenas os y's sao utilizados*/
			y = vld2_u8(pointer_y);
			y0 = vmovl_u8(y.val[0]);
			/* combina os dois vetores de maneira a intercalar os valores
			 * [cb0,cb0,cb1,cb1,...] e [cr0,cr0,cr1,cr1,...]*/
			if(i == 0){
				utemp = vzip_s16(vget_low_s16(yuv1),vget_low_s16(yuv1));
				vtemp = vzip_s16(vget_low_s16(yuv3),vget_low_s16(yuv3));
			}else{
				utemp = vzip_s16(vget_high_s16(yuv1),vget_high_s16(yuv1));
				vtemp = vzip_s16(vget_high_s16(yuv3),vget_high_s16(yuv3));
			}
			
			u = vcombine_s16(utemp.val[0],utemp.val[1]);
			v = vcombine_s16(vtemp.val[0],vtemp.val[1]);
			
			//faz o calculo do v1
			vIntermediario = vaddq_s16(v,vector128);
			uIntermediario = vaddq_s16(u,vector128);
			v1 = vshrq_n_s16(vaddq_s16(vshlq_n_s16(vIntermediario,1),vIntermediario),1);
			
			
			//faz o calculo do u1
			u1 = vshrq_n_s16(vaddq_s16(vshlq_n_s16(uIntermediario,7),uIntermediario),6);
			
			//faz o calculo do rg = (((u - 128) << 1) +  (u - 128) + ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
			rgIntermediario = vaddq_s16(vshlq_n_s16(vIntermediario,2),vshlq_n_s16(vIntermediario,1));
			rg = vshrq_n_s16(vaddq_s16(vaddq_s16(vshlq_n_s16(uIntermediario,1),uIntermediario),rgIntermediario),3);
			

			//faz a soma e a saturacao
			red = vmaxq_s16(vector0,vminq_s16(vector255,vaddq_s16(y0,v1)));
			green = vmaxq_s16(vector0,vminq_s16(vector255,vsubq_s16(y0,rg)));
			blue = vmaxq_s16(vector0,vminq_s16(vector255,vaddq_s16(y0,u1)));
			
			
			convert = vmovn_u16(red);
			rgb.val[0] = convert;
			
			convert = vmovn_u16(green);
			rgb.val[1] = convert;
			
			convert = vmovn_u16(blue);
			rgb.val[2] = convert;
			
			vst3_u8(vdest,rgb);
			
			//move o ponteiro destino (rgb)
			vdest+=24;
			
			//move o ponteiro do vetor yuv
			pointer_y+=16;
			
		}
		
		//move o ponteiro do auxiliar
		pointer_cbcr+=32;
    }
}


/**
 * Funcao que converte YUV4:2:2 para RGB24 utilizando as instrucoes NEON porem substituindo um load da memoria
 * por computacao de dados (utilizando intercalacao)
 * */

void v4lconvert_yuyv_to_rgb24_optimized_NEON_test2(const unsigned char *src, unsigned char *dest,
  int width, int height)
{
	int j,i=0;
	int x;
	int screen_size_pixels = width*height;//1280x720
	int screen_size_bytes = screen_size_pixels*2;
	
	//uint8_t *pointer_y = src;
	const uint8_t *pointer_cbcr = src;
	uint8_t *vdest = dest; //ponteiro para gravar no destino
	
	uint8x8x4_t yuyv; //armazena os 4 componentes porem utiliza apenas os cbs e crs
	uint8x8x3_t rgb; //armazena os 3 componentes finais para a gravacao
	
	int16x4x2_t utemp; //variavel temporaria para o u, utilizada para intercalar os valores
	int16x4x2_t vtemp; //variavel temporaria para o v, utilizada para intercalar os valores
	int16x4x2_t ytemp;

	int16x8_t u; //u ou cb
	int16x8_t v; //v ou cr
	int16x8_t y;
	
	int16x8_t vIntermediario; //armazena valores intermediarios
	int16x8_t uIntermediario;//armazena valores intermediarios
	int16x8_t v1;
	int16x8_t u1;
	int16x8_t rgIntermediario;//armazena valores intermediarios
	int16x8_t rg;
	int16x8_t red;
	int16x8_t green;
	int16x8_t blue;
	uint8x8_t convert;
	uint8x8x2_t final;
	
	/* vetores de constantes*/
	int16x8_t vector128 = vdupq_n_s16(-128);
	int16x8_t vector0 = vdupq_n_s16(0);
	int16x8_t vector255 = vdupq_n_s16(255);
	
	int16x8_t yuv0;
	int16x8_t yuv1;
	int16x8_t yuv2;
	int16x8_t yuv3; 
    for (j = 0; j < screen_size_bytes; j += 32) {

		/* Apenas os cbs e crs sao utilizados*/
		yuyv = vld4_u8(pointer_cbcr);
		
		yuv0 = vmovl_u8(yuyv.val[0]); //y0,y2,y4,...,y14
		yuv1 = vmovl_u8(yuyv.val[1]); //cbs
		yuv2 = vmovl_u8(yuyv.val[2]); //y1,y3,y5,...,y15
		yuv3 = vmovl_u8(yuyv.val[3]); //crs
		
		for(i = 0;i<2;i++){
			rgb = vld3_u8(vdest); //carrega rgb para os registradores (3)
			/* Apenas os y's sao utilizados*/
			//y = vld2_u8(pointer_y);
			//y0 = vmovl_u8(y.val[0]);
			/* combina os dois vetores de maneira a intercalar os valores
			 * [cb0,cb0,cb1,cb1,...] e [cr0,cr0,cr1,cr1,...]*/
			if(i == 0){
				utemp = vzip_s16(vget_low_s16(yuv1),vget_low_s16(yuv1));
				vtemp = vzip_s16(vget_low_s16(yuv3),vget_low_s16(yuv3));
				ytemp = vzip_s16(vget_low_s16(yuv0),vget_low_s16(yuv2));
			}else{
				utemp = vzip_s16(vget_high_s16(yuv1),vget_high_s16(yuv1));
				vtemp = vzip_s16(vget_high_s16(yuv3),vget_high_s16(yuv3));
				ytemp = vzip_s16(vget_high_s16(yuv0),vget_high_s16(yuv2));
			}
			
			u = vcombine_s16(utemp.val[0],utemp.val[1]);
			v = vcombine_s16(vtemp.val[0],vtemp.val[1]);
			y = vcombine_s16(ytemp.val[0],ytemp.val[1]);
			
			//faz o calculo do v1
			vIntermediario = vaddq_s16(v,vector128);
			uIntermediario = vaddq_s16(u,vector128);
			v1 = vshrq_n_s16(vaddq_s16(vshlq_n_s16(vIntermediario,1),vIntermediario),1);
			
			
			//faz o calculo do u1
			u1 = vshrq_n_s16(vaddq_s16(vshlq_n_s16(uIntermediario,7),uIntermediario),6);
			
			//faz o calculo do rg = (((u - 128) << 1) +  (u - 128) + ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
			rgIntermediario = vaddq_s16(vshlq_n_s16(vIntermediario,2),vshlq_n_s16(vIntermediario,1));
			rg = vshrq_n_s16(vaddq_s16(vaddq_s16(vshlq_n_s16(uIntermediario,1),uIntermediario),rgIntermediario),3);
			

			//faz a soma e a saturacao
			red = vmaxq_s16(vector0,vminq_s16(vector255,vaddq_s16(y,v1)));
			green = vmaxq_s16(vector0,vminq_s16(vector255,vsubq_s16(y,rg)));
			blue = vmaxq_s16(vector0,vminq_s16(vector255,vaddq_s16(y,u1)));
			
			
			convert = vmovn_u16(red);
			rgb.val[0] = convert;
			
			convert = vmovn_u16(green);
			rgb.val[1] = convert;
			
			convert = vmovn_u16(blue);
			rgb.val[2] = convert;
			
			vst3_u8(vdest,rgb);
			
			//move o ponteiro destino (rgb)
			vdest+=24;
			
			//move o ponteiro do vetor yuv
			//pointer_y+=16;
			
		}
		
		//move o ponteiro do auxiliar
		pointer_cbcr+=32;
    }
}
